//
// Created by root on 22-10-28.
//

#include "visualInertialOdometry.h"




namespace VisualInertialOdometry{

    VIO::VIO(){
        CAMERA_INIT=trackData.readIntrinsicParameter(CAM_MODEL);
        assert(CAMERA_INIT);
    }


    bool VIO::processImage(const sensor_msgs::ImageConstPtr &img_msg) {
        if(!CAMERA_INIT){
            cout<<"camera model not initialized! Try again later "<<endl;
            return false;
        }
        ros::NodeHandle n("~");
        cv_bridge::CvImageConstPtr ptr;
        if (img_msg->encoding == "8UC1")
        {
            sensor_msgs::Image img;
            img.header = img_msg->header;
            img.height = img_msg->height;
            img.width = img_msg->width;
            img.is_bigendian = img_msg->is_bigendian;
            img.step = img_msg->step;
            img.data = img_msg->data;
            img.encoding = "mono8";
            ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
        }
        else
            ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
        cv::Mat show_img = ptr->image;
        trackData.track(ptr->image,img_msg->header.stamp.toSec());
        trackData.updateID();
        //发布ROSMSG点云信息
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
        sensor_msgs::ChannelFloat32 id_of_point;
        sensor_msgs::ChannelFloat32 u_of_point;
        sensor_msgs::ChannelFloat32 v_of_point;
        sensor_msgs::ChannelFloat32 velocity_x_of_point;
        sensor_msgs::ChannelFloat32 velocity_y_of_point;

        feature_points->header = img_msg->header;
        feature_points->header.frame_id = "world";
        set<int> hash_ids;
        auto &un_pts = trackData.cur_un_pts;
        auto &cur_pts = trackData.cur_pts;
        auto &ids = trackData.ids;
        auto &pts_velocity = trackData.pts_velocity;
        for (unsigned int j = 0; j < ids.size(); j++){
            if (trackData.track_cnt[j] > 1)
            {
                int p_id = ids[j];
                hash_ids.insert(p_id);
                geometry_msgs::Point32 p;
                p.x = un_pts[j].x;
                p.y = un_pts[j].y;
                p.z = 1;
                feature_points->points.push_back(p);
                id_of_point.values.push_back(p_id);
                u_of_point.values.push_back(cur_pts[j].x);
                v_of_point.values.push_back(cur_pts[j].y);
                velocity_x_of_point.values.push_back(pts_velocity[j].x);
                velocity_y_of_point.values.push_back(pts_velocity[j].y);
            }
        }
        pub_img=n.advertise<sensor_msgs::PointCloud>("feature", 1000);
        feature_points->channels.push_back(id_of_point);
        feature_points->channels.push_back(u_of_point);
        feature_points->channels.push_back(v_of_point);
        feature_points->channels.push_back(velocity_x_of_point);
        feature_points->channels.push_back(velocity_y_of_point);
        pub_img.publish(feature_points);
        for(int i=0;i<trackData.forw_pts.size();i++)
                cv::circle(ptr->image,Point(trackData.forw_pts[i].x,trackData.forw_pts[i].y),6.5,Scalar(255,255,255));
        cv::imshow("result",ptr->image);
        waitKey(30);
    }


    void VIO::process() {
        //ros::NodeHandle n("~");
        //cout<<"to do ...."<<endl;
 //       pub_img=n.advertise<sensor_msgs::PointCloud>("feature", 1000);
//        while(true){
//            if(VIO::PUB_THIS_FRAME){
//
//            }
//        }


        //int cur_index=STARTIN,end_index=ENDIN;

//        while(cur_index<end_index){
//
//            Mat image=imread(PREFIX+ to_string(cur_index)+".png");
//            //光流提取角点
//            trackData.track(image);
//            //更新特征点ID
//            trackData.updateID();
//            //发布ROSMSG点云信息
//            sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
//            sensor_msgs::ChannelFloat32 id_of_point;
//            sensor_msgs::ChannelFloat32 u_of_point;
//            sensor_msgs::ChannelFloat32 v_of_point;
//            sensor_msgs::ChannelFloat32 velocity_x_of_point;
//            sensor_msgs::ChannelFloat32 velocity_y_of_point;
//
//            //feature_points->header = img_msg->header;
//            feature_points->header.frame_id = "world";
//            set<int> hash_ids;
//            auto &un_pts = trackData.cur_un_pts;
//            auto &cur_pts = trackData.cur_pts;
//            auto &ids = trackData.ids;
//            auto &pts_velocity = trackData.pts_velocity;
//            for (unsigned int j = 0; j < ids.size(); j++)
//            {
//                if (trackData.track_cnt[j] > 1)
//                {
//                    int p_id = ids[j];
//                    hash_ids.insert(p_id);
//                    geometry_msgs::Point32 p;
//                    p.x = un_pts[j].x;
//                    p.y = un_pts[j].y;
//                    p.z = 1;
//                    feature_points->points.push_back(p);
//                    id_of_point.values.push_back(p_id);
//                    u_of_point.values.push_back(cur_pts[j].x);
//                    v_of_point.values.push_back(cur_pts[j].y);
//                    velocity_x_of_point.values.push_back(pts_velocity[j].x);
//                    velocity_y_of_point.values.push_back(pts_velocity[j].y);
//                }
//            }
//            feature_points->channels.push_back(id_of_point);
//            feature_points->channels.push_back(u_of_point);
//            feature_points->channels.push_back(v_of_point);
//            feature_points->channels.push_back(velocity_x_of_point);
//            feature_points->channels.push_back(velocity_y_of_point);
//            pub_img.publish(feature_points);
//
//            for(int i=0;i<trackData.forw_pts.size();i++)
//                circle(image,Point(trackData.forw_pts[i].x,trackData.forw_pts[i].y),6.5,Scalar(0,0,255));
//            imshow("result",image);
//            waitKey(30);
//            cur_index++;
//        }
//        destroyWindow("result");
    }

}
